#include "stm32f4xx.h"
#include "pid.h"
#include "led.h"
#include "structconfig.h"
#include "control.h"
#include "imu.h"
#include "altitude.h"
/*****************************************************************************
*函  数：void PID_(PID_TYPE*PID,float target,float measure)
*功  能：位置式PID算法
*参  数：PID: 算法P I D参数的结构体
*        target: 目标值
*        measure: 测量值 
*返回值：无 
*备  注: 
*****************************************************************************/


////////////////////////////ROL角度外环控制////////////////////
void PID_Rol(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	PID->Differ = PID->Error - PID->PreError;    //微分量
	//PID->Differ =	-Gyr_rad.Y*RadtoDeg;    //微分量
	if(highaim>45)    //飞机解锁之后再加入积分,防止积分过调
	{
		{
			PID->Integral += PID->Error;                        //对误差进行积分
			if(PID->Integral > PID->Irang)                      //积分限幅
          PID->Integral = PID->Irang;
		  if(PID->Integral < -PID->Irang)                     //积分限幅
			    PID->Integral = -PID->Irang;                    
		}
	}
	else
	{PID->Integral = 0;}
	
	//PID->Pout = (PID->P + 0.04f*fabs(PID->Error)) * PID->Error;     //比例控制:动态P
	PID->Pout = PID->P * PID->Error;     //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制

	PID->PreError = PID->Error ;                            //前一个误差值
}

////////////////////////////PIT角度外环控制////////////////////
void PID_Pit(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	PID->Differ = PID->Error - PID->PreError;    //微分量
	//PID->Differ =	-Gyr_rad.X*RadtoDeg;    //微分量
	if(highaim>45)    //飞机解锁之后再加入积分,防止积分过调
	{
		{
			PID->Integral += PID->Error;                        //对误差进行积分
			if(PID->Integral > PID->Irang)                      //积分限幅
          PID->Integral = PID->Irang;
		  if(PID->Integral < -PID->Irang)                     //积分限幅
			    PID->Integral = -PID->Irang;                    
		}
	}
	else
	{PID->Integral = 0;}
	
	//PID->Pout = (PID->P + 0.04f*fabs(PID->Error)) * PID->Error;     //比例控制:动态P
	PID->Pout = PID->P * PID->Error;     //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制

	PID->PreError = PID->Error ;                            //前一个误差值
}

////////////////////////////偏航角控制////////////////////
void PID_Yaw(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	if(PID->Error>180) PID->Error=PID->Error-360;
	if(PID->Error<-180)PID->Error=PID->Error+360;

	PID->Differ = PID->Error - PID->PreError;    //微分量
	//PID->Differ = -Gyr_rad.Z*RadtoDeg;    //微分量
	
	if(Altitude>40&&yaw==0)    //飞机解锁之后再加入积分,防止积分过调
	{
		{
			PID->Integral += PID->Error;                        //对误差进行积分
			if(PID->Integral > PID->Irang)                      //积分限幅
          PID->Integral = PID->Irang;
		  if(PID->Integral < -PID->Irang)                     //积分限幅
			    PID->Integral = -PID->Irang;                    
		}
	}
	else
	{PID->Integral = 0;}
	
	//PID->Pout = (PID->P + 0.03f*fabs(PID->Error)) * PID->Error;                        //比例控制
	PID->Pout = PID->P * PID->Error;                        //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制
	
	PID->PreError = PID->Error ;                            //前一个误差值
}


////////////////////////////角速度内环控制////////////////////
void PID_Angle_R(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	PID->Differ = PID->Error - PID->PreError;    //微分量
	
	if(highaim>45&&Uav_Enable)    //飞机解锁之后再加入积分,防止积分过调
	{
		{
			PID->Integral += PID->Error;                        //对误差进行积分
			if(PID->Integral > PID->Irang)                      //积分限幅
          PID->Integral = PID->Irang;
		  if(PID->Integral < -PID->Irang)                     //积分限幅
			    PID->Integral = -PID->Irang;                    
		}
	}
	else
	{PID->Integral = 0;}
	
	PID->Pout = PID->P * PID->Error;                        //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制

	PID->PreError = PID->Error ;                            //前一个误差值
}

void PID_Yaw_R(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	PID->Differ = PID->Error - PID->PreError;    //微分量
	
	if(Altitude>40&&yaw==0)    //飞机解锁之后再加入积分,防止积分过调
	{
		{
			PID->Integral += PID->Error*0.01f;                        //对误差进行积分
			if(PID->Integral > PID->Irang)                      //积分限幅
          PID->Integral = PID->Irang;
		  if(PID->Integral < -PID->Irang)                     //积分限幅
			    PID->Integral = -PID->Irang;                    
		}
	}
	else
	{PID->Integral = 0;}
	
	PID->Pout = PID->P * PID->Error;                        //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制

	PID->PreError = PID->Error ;                            //前一个误差值
}

void PID_YAW_C(PID_TYPE*PID,float target,float measure)
{

	PID->Error  = target - measure;     //误差
	
	PID->OutPut = PID->P * PID->Error;    //比例控制

}

////////////////////////////高度控制////////////////////
void PID_High(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	PID->Differ = -V_Z;    //微分量
	
	if(highaim>45)    //飞机解锁之后再加入积分,防止积分过调
	{
		{
			PID->Integral += PID->Error;                        //对误差进行积分
			if(PID->Integral > PID->Irang)                      //积分限幅
          PID->Integral = PID->Irang;
		  if(PID->Integral < -PID->Irang)                     //积分限幅
			    PID->Integral = -PID->Irang;                    
		}
	}
	else
	{PID->Integral = 0;}
	
	//PID->Pout = (PID->P + 0.01f*fabs(PID->Error)) * PID->Error;     //比例控制:动态P
	PID->Pout = PID->P * PID->Error;                        //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制

	PID->PreError = PID->Error ;                            //前一个误差值
}

////////////////////////////高度内环控制////////////////////
void PID_High_R(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	//PID->Differ = PID->Error - PID->PreError;    //微分量
	PID->Differ = - a_z*100;
	if(Uav_Enable)    //飞机解锁之后再加入积分,防止积分过调
	{
		{
			PID->Integral += PID->Error;                        //对误差进行积分
			if(PID->Integral > PID->Irang)                      //积分限幅
          PID->Integral = PID->Irang;
		  if(PID->Integral < -PID->Irang)                     //积分限幅
			    PID->Integral = -PID->Irang;                    
		}
	}
	else
	{PID->Integral = 0;}
	
	PID->Pout = PID->P * PID->Error;                        //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制
//	if(PID->OutPut>100) PID->OutPut=100;											//输出限幅 输出到姿态外环
//	if(PID->OutPut<-100) PID->OutPut=-100;
	
	PID->PreError = PID->Error ;                            //前一个误差值
}

////////////////////////////位置外环控制////////////////////
void PID_Position(PID_TYPE*PID,float target,float measure)
{
	PID->Error  = target - measure;              //误差
	
	if(Uav_Enable)    //飞机解锁之后再加入积分,防止积分过调
	{
		{
			PID->Integral += PID->Error;                        //对误差进行积分
			if(PID->Integral > PID->Irang)                      //积分限幅
          PID->Integral = PID->Irang;
		  if(PID->Integral < -PID->Irang)                     //积分限幅
			    PID->Integral = -PID->Irang;                    
		}
	}
	else
	{PID->Integral = 0;}
	
	PID->Pout = PID->P * PID->Error;                        //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	
	PID->OutPut =  PID->Pout + PID->Iout ;       //比例 + 积分 + 微分总控制

}

int rpflag=0,rpflag_1=0,raim=0,paim=0,rpfinash=0,pfinish=0,rptime=0,rpt=0;
float V_Stop_Add,V_X_Stop,V_Y_Stop;

////////////////////////////位置环控制////////////////////
void PID_Position_X(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	PID->Differ = (PID->Error - PID->PreError)*100;      //加速度取代微分量
//	PID->Integral = - Position.X; //位置取代积分量
	
	
	rpflag_1=rpflag;
	if(V_X_Aim!=0||V_Y_Aim!=0) rpflag=1;//遥控器打杆时 积分分离 
		else  rpflag=0;
	
	V_Stop_Add=fabs(V.X)+fabs(V.Y); //总速度
	if(rpflag_1==1&&rpflag==0) rpfinash=1;  //打杆结束刹车
	if(rpfinash==1&&V_Stop_Add<20) {rpfinash=0;rpt=1;}
	if(rpt==1) rptime++;  
	if(rptime==30) {rpt=0;rptime=0;}//刹车后缓冲时间

	if(rpflag==1||rpfinash==1||rpt==1) //正在打杆状态 
	{
		Opt_Position.X=0;
		Opt_Position.Y=0;
		Position.X=0;
		Position.Y=0;
	}
	
	if(rpflag==1||rpfinash==1||rpt==1)    //飞机解锁之后再加入积分,防止积分过调
		{
				PID->Integral += PID->Error*0.01f;                        //对误差进行积分
				if(PID->Integral > PID->Irang)                      //积分限幅
						PID->Integral = PID->Irang;
				if(PID->Integral < -PID->Irang)                     //积分限幅
						PID->Integral = -PID->Irang;                    
		}
		else
		{PID->Integral = 0;}
	
	PID->Pout = PID->P * PID->Error;                       //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制
	
//	PID->OutPut =  atan((PID->Pout + PID->Iout + PID->Dout)/980)*RadtoDeg;     //速度输出转角度 arctan(速度/G)*RadtoDeg
	
//	if(rpfinash&&V.X>0) PID->OutPut = -1.2f*sqrt(fabs(V.X)*cos(Att_Angle.rol*DegtoRad ));//刹车控制 使用刹车函数进行刹车控制
//	if(rpfinash&&V.X<0) PID->OutPut =  1.2f*sqrt(fabs(V.X)*cos(Att_Angle.rol*DegtoRad ));
//	if(rpflag) PID->OutPut= 0.2f*PID->Pout - a_x*2; 		//打杆时
	
	
	PID->PreError = PID->Error ;                            //前一个误差值

}

void PID_Position_Y(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	//PID->Differ = PID->Error - PID->PreError;    //微分量
	PID->Differ = (PID->Error - PID->PreError)*100;   //加速度取代微分量
	
//	PID->Integral = - Position.Y;  //位置取代积分量

	if(rpflag==1||rpfinash==1||rpt==1)    //飞机解锁之后再加入积分,防止积分过调
		{
				PID->Integral += PID->Error*0.01f;                        //对误差进行积分
				if(PID->Integral > PID->Irang)                      //积分限幅
						PID->Integral = PID->Irang;
				if(PID->Integral < -PID->Irang)                     //积分限幅
						PID->Integral = -PID->Irang;                    
		}
		else
		{PID->Integral = 0;}

	
	PID->Pout = PID->P * PID->Error;                        //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制
	
//	PID->OutPut =  atan((PID->Pout + PID->Iout + PID->Dout)/980)*RadtoDeg; //速度输出转角度 arctan(速度/G)*RadtoDeg
	
//	if(rpfinash&&V.Y>0) PID->OutPut = -1.2f*sqrt(fabs(V.Y)*cos(Att_Angle.pit*DegtoRad ));//刹车控制
//	if(rpfinash&&V.Y<0) PID->OutPut =  1.2f*sqrt(fabs(V.Y)*cos(Att_Angle.pit*DegtoRad ));
//	if(rpflag) PID->OutPut= 0.2f*PID->Pout - a_y*2; 		//打杆时 
	

	
	PID->PreError = PID->Error ;                            //前一个误差值
}

////////////////////////////位置最内环控制////////////////////
void PID_Position_ax(PID_TYPE*PID,float target,float measure)
{
	PID->Error  = target - measure;              //误差
	
	PID->Pout = PID->P * PID->Error;                        //比例控制
	
//	PID->OutPut =  PID->Pout  ;       //比例 + 积分 + 微分总控制
	
	PID->OutPut =  atan((PID->Pout)/980)*RadtoDeg; //速度输出转角度 arctan(速度/G)*RadtoDeg
	
	if(rpflag==1||rpfinash==1||rpt==1) PID->OutPut=0.20f*PID_POSITION_X.Error+ 0.09f*PID_POSITION_X.Integral +0.04f*PID_POSITION_X.Differ; //打杆时
	
	if(PID->OutPut>20) PID->OutPut=20;											//输出限幅 输出到姿态外环
	if(PID->OutPut<-20) PID->OutPut=-20;
}

void PID_Position_ay(PID_TYPE*PID,float target,float measure)
{
	PID->Error  = target - measure;              //误差
	
	PID->Pout = PID->P * PID->Error;                        //比例控制
	
//	PID->OutPut =  PID->Pout  ;       //比例 + 积分 + 微分总控制
	
	PID->OutPut =  atan((PID->Pout)/980)*RadtoDeg; //速度输出转角度 arctan(速度/G)*RadtoDeg
	
	if(rpflag==1||rpfinash==1||rpt==1) PID->OutPut=0.20f*PID_POSITION_Y.Error+ 0.09f*PID_POSITION_Y.Integral +0.04f*PID_POSITION_Y.Differ; //打杆时
	
	if(PID->OutPut>20) PID->OutPut=20;											//输出限幅 输出到姿态外环
	if(PID->OutPut<-20) PID->OutPut=-20;
}

